'''
Author: GH
Date: 2021-07-25 21:21:05
LastEditTime: 2021-07-25 21:25:00
LastEditors: Please set LastEditors
Description: In User Settings Edit
FilePath: \FFTfly\MyOpenmv\find_vertical.py
'''
#在寻找直线与直角时，由于直角与直线的阈值相差较大，因此可直接通过两阈值的比较来实现寻找直角的目标

import sensor, image, time
from pyb import LED
from pyb import UART,Timer

uart = UART(3,115200)#初始化串口 波特率 115200
sensor.reset()
#sensor.set_vflip(True)
#sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time = 2000)     # WARNING: If you use QQVGA it may take seconds
clock = time.clock()                # to process a frame sometimes.

x_width = 80
y_height = 60    # 图像是QQQVGA,则图像是80x60的分辨率

class singleline_check():
    flager = 0
    rho_err = 0
    theta_err = 0
    is_verticle = 0

singleline_check = singleline_check()

BINARY_THRESHOLD = (30, 120)  #二值化阈值

verticle_pixels_threshold = [200, 300]   #像素最大和最小阈值
track_line_threshold = [100, 200]

def count_pixels_with_movement(img):
    global x_width, y_height

    x_pos = 0
    y_pos = 0
    total_white_pixels = 0
    for x_pos in range(x_width):
        for y_pos in range(y_height):
            if img.get_pixel(x_pos, y_pos) == 255:
                total_white_pixels += 1  # 利用get_pixel()方法，计算当前图像中白色色块所占的像素大小

    print("total white pixels are", total_white_pixels)
    if total_white_pixels >= verticle_pixels_threshold[0] and total_white_pixels <= verticle_pixels_threshold[1]:
        singleline_check.is_verticle = 2
    elif total_white_pixels >= track_line_threshold[0] and total_white_pixels <= track_line_threshold[1]:
        singleline_check.is_verticle = 1
    else:
        singleline_check.is_verticle = 0

    print("is verticle flag:", singleline_check.is_verticle)

while True:
    img = sensor.snapshot().binary([BINARY_THRESHOLD]).erode(1)
    count_pixels_with_movement(img)

    # 之后打包发送数据给飞控，这里略...
